/*
 * Copyright 2025 The Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *@file    : velocity_factor.h
 *@brief   : velocity factor
 *@authors : zhanglei
 *@version : v1.0
 *@data    : 2025/10/1
 *
 */

#ifndef VELOCITY_FACTOR_H
#define VELOCITY_FACTOR_H

#include <ceres/ceres.h>

#include "common/state.h"

namespace optimizer {

class VelocityFactor : public ceres::SizedCostFunction<2, 2>{
public:  
   VelocityFactor(Eigen::Vector2d& world_velocity) : world_velocity_(world_velocity) {
      cov_ = Eigen::Matrix<double, 2, 2>::Identity();
      cov_(0,0) = 0.01;
      cov_(1,1) = 0.01;
   }

   // ceres-solver
   // parameters[0] : bg, ba
   virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
 
      Eigen::Vector2d velocity(parameters[0][0], parameters[0][1]);

      Eigen::Map<Eigen::Matrix<double, 2, 1>> residual(residuals);

      residual = velocity - world_velocity_;

      // LOG(INFO) << "bg " << bg << ", ba : " << ba.transpose() << std::endl;
      // LOG(INFO) << "residual " << (residual).transpose() << std::endl;
      // LOG(INFO) << "imu_preintegration_->cov_ \n " << imu_preintegration_->cov_ << std::endl;

      Eigen::Matrix<double, 2, 2> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 2, 2>>(cov_.inverse()).matrixL().transpose();
      residual = sqrt_info * residual;

      if (jacobians) {
         if (jacobians[0]) {
            Eigen::Map<Eigen::Matrix<double, 2, 2, Eigen::RowMajor>> jacobian_velocity(jacobians[0]);
            jacobian_velocity.setIdentity(); 

            jacobian_velocity = sqrt_info * jacobian_velocity;
         }
      }

      return true;
   }

private:
   Eigen::Matrix<double, 2, 2> cov_;
   Eigen::Vector2d world_velocity_;
};

}

#endif 
